
#include "fur_control/fur_control.hpp"

ur_control::ur_control() : rclcpp::Node("fur_control_node")
{   
    // 创建关节状态订阅者
    robot_state_subscriber_ = this->create_subscription<sensor_msgs::msg::JointState>(
        "/joint_states", rclcpp::SystemDefaultsQoS(), std::bind(&ur_control::get_joint_states, this, std::placeholders::_1)
    );
    // 创建关节控制命令发布者
    robot_command_publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>(
        controller_name_ + "/commands", rclcpp::SystemDefaultsQoS()
    );
    // 初始化目标位置姿态
    target_quaterniond_.x() = 0.5;
    target_quaterniond_.y() = 0.5;
    target_quaterniond_.z() = 0.5;
    target_quaterniond_.w() = 0.5;
    target_position_ = {0.4, 0.0, 0.5};

    // pinocchion初始化
    //加载模型
    pinocchio::urdf::buildModel(urdf_filename_, fur_model_);
    pinocchio::Data fur_data(fur_model_);

    // 定时发布关节控制命令
    // 创建定时器
    timer_ = this->create_wall_timer(10ms, std::bind(&ur_control::publish_commands, this));

    // keyboard
    loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05, std::bind(&ur_control::KeyboardInterface, this));
    loop_keyboard->start();

}

ur_control::~ur_control()
{
    loop_keyboard->shutdown();
};

void ur_control::InverseKinematics(Eigen::Quaterniond target_quater, Eigen::Vector3d target_pos, pinocchio::Model model)
{
    // 配置迭代参数
    const int JOINT_ID = 6;
    Eigen::Matrix<double, 1, 6> pos;
    for (int i=0; i<6; i++)
    {
        pos[i] = joints_now_pos_[i];
    }
    Eigen::VectorXd q = pos.transpose();
    const double eps = 1e-6;
    const int IT_MAX = 1000;
    const double DT = 0.025;
    const double damp = 1e-3;
    // 加载数据
    pinocchio::Data data(model);
    // //设定雅可比矩阵
    pinocchio::Data::Matrix6x J(6, model.nv);
    Eigen::VectorXd v(model.nv);
    bool success = false;
    // //设置目标位姿
    std::vector<pinocchio::SE3> targets {pinocchio::SE3(target_quater, target_pos),};
    std::cout<<target_pos<<std::endl;
    //开始利用雅可比矩阵迭代
    for (int idx = 0; idx < targets.size(); ++idx)
    {
        for (int i = 0;; i++)
        {
            //正运动学更新
            pinocchio::forwardKinematics(model, data, q);
            // 获取误差矩阵
            const pinocchio::SE3 iMd = data.oMi[JOINT_ID].actInv(targets[idx]);
            // 将误差矩阵转化为误差
            Eigen::Matrix<double, 6, 1> err = pinocchio::log6(iMd).toVector();  // in joint frame
            if (err.norm() < eps)
            {
                success = true;
                break;
            }
            if (i >= IT_MAX)
            {
                success = false;
                break;
            }
            // 获取雅可比矩阵
            pinocchio::computeJointJacobian(model, data, q, JOINT_ID, J);
            pinocchio::Data::Matrix6 Jlog;
            //对雅可比矩阵的值进行修正
            pinocchio::Jlog6(iMd.inverse(), Jlog);
            J = -Jlog * J;
            pinocchio::Data::Matrix6 JJt;
            JJt.noalias() = J * J.transpose();
            JJt.diagonal().array() += damp;
            //获取关节角度微分
            v.noalias() = -J.transpose() * JJt.ldlt().solve(err);
            //更新关节角度值
            q = pinocchio::integrate(model, q, v * DT);
        }
    }
    // 更新命令关节角度
    Eigen::Matrix<double, 6, 1> p = q.transpose();
    for (int i=0; i<6; i++)
    {
        joints_command_pos_[i] = p[i];
    }
}

void ur_control::publish_commands()
{
    std_msgs::msg::Float64MultiArray command_msg;
    // 通过目标位姿解算关节角度
    InverseKinematics(target_quaterniond_, target_position_, fur_model_);
    command_msg.data = joints_command_pos_;
    robot_command_publisher_->publish(command_msg);
}

void ur_control::get_joint_states(const sensor_msgs::msg::JointState::SharedPtr joint_states)
{
    // 获取当前关节角度
    joints_now_pos_ = joint_states->position;
}

#include <termios.h>
#include <sys/ioctl.h>
static bool kbhit()
{
    termios term;
    tcgetattr(0, &term);

    termios term2 = term;
    term2.c_lflag &= ~ICANON;
    tcsetattr(0, TCSANOW, &term2);

    int byteswaiting;
    ioctl(0, FIONREAD, &byteswaiting);

    tcsetattr(0, TCSANOW, &term);

    return byteswaiting > 0;
}

void ur_control::KeyboardInterface()
{
    if (kbhit())
    {
        int c = fgetc(stdin);
        switch (c)
        {
        case 'i':
            target_position_(2, 0) += 0.01;
            break;
        case 'k':
            target_position_(2, 0) -= 0.01;
            break;
        case 'j':
            target_position_(1, 0) += 0.01;
            break;
        case 'l':
            target_position_(1, 0) -= 0.01;
            break;
        case 'm':
            target_position_(0, 0) += 0.01;
            break;
        case ',':
            target_position_(0, 0) -= 0.01;
            break;
        default:
            break;
        }
    }
}

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ur_control>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}


